#include "Tracing.h"
#include "Motor.h"
#include "Control.h"
#include "ti_msp_dl_config.h"
#include "bsp_mpu6050.h"
volatile int32_t Sum_Pulse;
volatile int32_t PWM;
int32_t Dir_out;
int32_t z_gyro_raw;
// 是否反转传感器逻辑：1 表示低电平是黑线
#define INVERT_SENSOR_LOGIC 0

// 获取循迹状态，按位返回每个传感器的高低电平（0~31）
uint8_t getTracingState(void)
{
    uint8_t state = 0;

#if INVERT_SENSOR_LOGIC
    #define READ(pin) (!DL_GPIO_readPins(TRACING_PORT, pin))
#else
    #define READ(pin) (DL_GPIO_readPins(TRACING_PORT, pin))
#endif

    state |= (READ(TRACING_TRAING_0_PIN) ? 1 << 0 : 0);
    state |= (READ(TRACING_TRAING_1_PIN) ? 1 << 1 : 0);
    state |= (READ(TRACING_TRAING_2_PIN) ? 1 << 2 : 0);
    state |= (READ(TRACING_TRAING_3_PIN) ? 1 << 3 : 0);
    state |= (READ(TRACING_TRAING_4_PIN) ? 1 << 4 : 0);

    return state;
}

float Calculation_error(void)
{
	float err = 0;
    static float last_err =0;
	int state = 0;//状态获取变量定义
    state = getTracingState();//获取传感器状态
  switch(state)
  {
    case 0b11000:err = 14;break;  //左偏
    case 0b11100:err = 8;break;   //左偏
    case 0b11110:err = 7;break;   //左偏
    case 0b11101:err = 3;break;  //左偏
    case 0b11001:err = 2;break;   //左偏

    case 0b11011:err = 0;break;   //中线

    case 0b10011:err = -2;break;  //右偏
    case 0b10111:err = -3;break;  //右偏
    case 0b01111:err = -7;break;  //右偏
    case 0b00111:err = -8;break;  //右偏
    case 0b00011:err = -14;break; //右偏

    default:
        err = last_err; break;
  }
    last_err = err;
    return err;
}

/**
 * @brief 基础循迹函数（使用速度 PID + 方向差分修正）
 */
void Tracking(int Set_speed)
{
    static int speed_count = 0;
    static int pos_count = 0;
    static int PWM = 0;         // 速度环输出
    int LALL_PWM = 0, RALL_PWM = 0;
    short gyro[3];
    MPU6050ReadGyro(gyro);
    z_gyro_raw = gyro[2];  // 原始数据
    float pos_error = Calculation_error();  // 位置误差（例如角度/距离偏差）
    Dir_out = wh_Turn_Out(pos_error,280,100 ); // 比例/积分控制得到速度差值
    // 左右轮速度修正（转向控制）
    LALL_PWM = 1000 - Dir_out-z_gyro_raw/20.4;
    RALL_PWM = 1000 + Dir_out+z_gyro_raw/20.4;
    // 输出到电机
    Set_Motor(LALL_PWM, RALL_PWM);
}

void TIMER_0_INST_IRQHandler(void)
{
	if(DL_TimerA_getPendingInterrupt(TIMER_0_INST))
	{
		if(DL_TIMER_IIDX_ZERO)
		{
			  // L_Pulse =L_Encoder_Pulse(Get_Encoder_countA) ; //左编码器
			  // R_Pulse =R_Encoder_Pulse(Get_Encoder_countB) ; //右编码器
         Tracking(1000);
        // Get_Encoder_countA = 0;
        // Get_Encoder_countB = 0;
                          // LED_Flash(100);//LED1闪烁
		}
		       
	}
}